﻿using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace NewAI.Tools
{
    class AStarPro
    {
        //结构体_mapPoint表示地图中点的横纵坐标
        private struct _mapPoint
        {
            public int x;
            public int y;

            public static _mapPoint Create(int x, int y)
            {
                _mapPoint temp = new _mapPoint();
                temp.x = x;
                temp.y = y;
                return temp;
            }


        }

        private class FindWay
        {
            //地图中单个点：Point类
            class Point
            {
                public Point() { }
                public Point(_mapPoint m)
                {
                    x = m.x;
                    y = m.y;
                }
                public int G;
                public int H;
                public int x;
                public int y;
                public Point father;
            }
            //定义“开启列表”泛型集合
            static List<Point> open_List = new List<Point>();
            //定义“关闭列表”泛型集合
            static List<Point> close_List = new List<Point>();

            static byte[,] map;

            public static List<_mapPoint> Astar(_mapPoint startPoint, _mapPoint endPoint, ref byte[,] map)
            {
                if (map == null)
                    return new List<_mapPoint>();
                FindWay.map = (byte[,])map.Clone();

                //判断开始点和结束点是否相同
                if (startPoint.x == endPoint.x && startPoint.y == endPoint.y)
                {
                    List<_mapPoint> temp1 = new List<_mapPoint>();
                    temp1.Add(_mapPoint.Create(startPoint.x, startPoint.y));
                    return temp1;
                }
                //判断是否超出范围
                if (!(startPoint.x >= 0 && startPoint.x < map.GetLength(0) && startPoint.y >= 0 && startPoint.y < map.GetLength(1)) ||
                    !(endPoint.x >= 0 && endPoint.x < map.GetLength(0) && endPoint.y >= 0 && endPoint.y < map.GetLength(1)))
                {
                    List<_mapPoint> temp2 = new List<_mapPoint>();
                    return temp2;
                }


                //定义出发位置
                Point startingPoint = new Point(startPoint);

                //定义目的地
                Point destination = new Point(endPoint);

                //将startingPoint添加到open_List泛型集合
                open_List.Add(startingPoint);

                //当终点存在开启列表中就意味着寻路结束了
                while (!(IsInOpenList(destination.x, destination.y)))
                {
                    //定义nowPoint为开启列表中F值最小的节点（目标节点）
                    Point nowPoint = GetMinFFromOpenList();

                    //将nowPoint从open_List泛型集合中删除
                    open_List.Remove(nowPoint);

                    //将nowPoint添加到close_List泛型集合
                    close_List.Add(nowPoint);

                    //检查是否存在nowPoint的下一邻居节点
                    CheckGrid(nowPoint, destination, ref map);
                }
                //定义“所走路线”泛型集合
                List<_mapPoint> way = new List<_mapPoint>();

                Point p = GetPointFromOpenList(destination.x, destination.y);
                //定义“被访问节点temp”泛型集合
                List<Point> temp = new List<Point>();
                //如果p点的父节点不为空
                while (p.father != null)
                {
                    //将p点添加到temp泛型集合
                    temp.Add(p);
                    //每个被访问的节点都会记录父节点
                    p = p.father;
                }
                //逆序遍历
                temp.Reverse();
                //确定所走的路线（将temp中的点放在路线中）
                foreach (Point pt in temp)
                {
                    way.Add(new _mapPoint() { x = pt.x, y = pt.y });
                }
                //移除“开启列表”中的所有元素
                open_List.Clear();
                //移除“关闭列表”中的所有元素
                close_List.Clear();
                //返回坦克要走的线路
                return way;

            }

            //获得在OpenList中的点
            private static Point GetPointFromOpenList(int x, int y)
            {
                foreach (Point p in open_List)
                    if (p.x == x && p.y == y)
                        return p;
                return null;
            }

            //检查当前节点附近的节点
            private static void CheckGrid(Point nowPoint, Point destination, ref byte[,] map)
            {

                if (nowPoint == null)
                {
                    throw new Exception("找不到路了。");
                }
                //两个for循环用于判断九宫格范围内的节点
                for (int xt = nowPoint.x - 1; xt <= nowPoint.x + 1; xt++)
                {
                    for (int yt = nowPoint.y - 1; yt <= nowPoint.y + 1; yt++)
                    {
                        //排除超过边界、不相干和关闭列表中的点
                        if (!(xt >= 0 && xt < map.GetLength(0) && yt >= 0 && yt < map.GetLength(1)))
                            continue;

                        if (IsBar(xt, yt)) continue;
                        if (IsInCloseList(xt, yt)) continue;
                        //排除障碍物


                        //排除对角线上的点
                        if ((xt == nowPoint.x - 1 && yt == nowPoint.y - 1) ||
                            (xt == nowPoint.x + 1 && yt == nowPoint.y - 1) ||
                            (xt == nowPoint.x - 1 && yt == nowPoint.y + 1) ||
                            (xt == nowPoint.x + 1 && yt == nowPoint.y + 1))
                            continue;

                        if (IsInOpenList(xt, yt))
                        {
                            Point pt = GetPointFromOpenList(xt, yt);
                            int cost = CalculateConsumption(ref map, pt, nowPoint);
                            int G_new = nowPoint.G + cost;
                            if (G_new < pt.G)
                            {
                                open_List.Remove(pt);
                                pt.father = nowPoint;
                                pt.G = G_new;
                                open_List.Add(pt);
                            }
                        }
                        else //不在开启列表中
                        {
                            Point pt = new Point();
                            pt.x = xt;
                            pt.y = yt;
                            pt.father = nowPoint;
                            int cost = CalculateConsumption(ref map, pt, nowPoint);
                            pt.G = pt.father.G + cost;
                            pt.H = Math.Abs(pt.x - destination.x) + Math.Abs(pt.y - destination.y);
                            open_List.Add(pt);
                        }
                    }
                }
            }
            //计算消耗
            private static int CalculateConsumption(ref byte[,] map, Point pt, Point nowPoint)
            {
                int xt = nowPoint.x;
                int yt = nowPoint.y;
                int cost = map[pt.x, pt.y] - map[xt, yt];
                if (map[xt, yt] == 1)

                    return cost = 1;
                if (map[xt, yt] == 2)
                    return cost = 2;
                else
                    return cost = 999999999;
            }

            //检查是否有障碍物
            private static bool IsBar(int xt, int yt)
            {

                if (map[xt, yt] == 0)
                {
                    return true;
                }
                return false;
            }

            //从开启列表查找F值最小的节点（F=G+H）
            //即确认一个点是否为目标节点
            private static Point GetMinFFromOpenList()
            {

                Point Pmin = null;
                foreach (Point p in open_List)
                    if (Pmin == null || Pmin.G + Pmin.H > p.G + p.H)
                        Pmin = p;
                return Pmin;
            }

            //判断开启列表是否包含一个坐标的点
            private static bool IsInOpenList(int x, int y)
            {
                //老谭注意：这有Lambda表达式
                Point p = open_List.Find(item => item.x == x && item.y == y);
                if (p != null)
                    return true;
                return false;
            }

            //判断关闭列表是否包含一个坐标的点
            private static bool IsInCloseList(int x, int y)
            {
                //老谭注意：这有Lambda表达式
                Point p = close_List.Find(item => item.x == x && item.y == y);
                if (p != null)
                    return true;
                return false;
            }
        }

        /// <summary>
        /// 使用A星算法计算最短路径，如果找不到路径将会抛出异常
        /// </summary>
        /// <param name="map">地图（0=不能行走，1=走路代价为1，2=走路代价2）</param>
        /// <param name="startPoint">起点</param>
        /// <param name="endPoint">终点</param>
        /// <param name="path">导航路径（含起点和终点）</param>
        public static void Astar(byte[,] map, Point startPoint, Point endPoint, out List<Point> path)
        {
            if (map == null)
                throw new ArgumentNullException("地图为空");
            if (map.GetLength(0) == 0 || map.GetLength(0) == 0)
                throw new ArgumentException("地图的长度或宽度不能为0");
            if (startPoint.X < 0 || startPoint.X > map.GetLength(0) - 1)
                throw new ArgumentOutOfRangeException("起点X坐标超出范围");
            if (endPoint.X < 0 || endPoint.X > map.GetLength(0) - 1)
                throw new ArgumentOutOfRangeException("终点X坐标超出范围");
            if (startPoint.Y < 0 || startPoint.Y > map.GetLength(1) - 1)
                throw new ArgumentOutOfRangeException("起点Y坐标超出范围");
            if (endPoint.Y < 0 || endPoint.Y > map.GetLength(1) - 1)
                throw new ArgumentOutOfRangeException("终点Y坐标超出范围");
            if (map[startPoint.X, startPoint.Y] == 0)
                throw new ArgumentException("起点在地图上不能走");
            if (map[endPoint.X, endPoint.Y] == 0)
                throw new ArgumentException("终点在地图上不能走");

            //TODO:还应该测试长宽不相等的情况
            path = new List<Point>();
            try
            {
                var result = FindWay.Astar(_mapPoint.Create(startPoint.X, startPoint.Y), _mapPoint.Create(endPoint.X, endPoint.Y), ref map);
                for (int i = 0; i < result.Count; i++)
                    path.Add(new Point(result[i].x, result[i].y));
            }
            catch (Exception)
            {
                throw new Exception("不能查找到最短路径");
            }
        }
    }
}

